#!/usr/bin/env python3
# coding:utf-8
import time
import threading

import rclpy
from rclpy.node import Node

from std_msgs.msg import String, Float64, Float64MultiArray, Int64, UInt64MultiArray
from nobloack_key_input import Noblock_terminal


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.pub_str = self.create_publisher(String, '/hz_string', 1)
        self.pub_int = self.create_publisher(Int64, '/hz_int', 1)
        self.pub_int_array = self.create_publisher(UInt64MultiArray, '/hz_int_array', 1)
        self.pub_float = self.create_publisher(Float64, '/hz_float', 1)
        self.pub_float_array = self.create_publisher(Float64MultiArray, '/hz_float_array', 1)

        self.delay_time = 0.1
        self.thread_init()
        self.dynamic_rate_pub()

    def thread_init(self):
        t1 = threading.Thread(target=self.change_delay_time, daemon=True)
        t1.start()


    def dynamic_rate_pub(self):
        data_str = 'hello world'
        data_int_array = [1, 2, 3, 4, 5]
        data_float_array = [1.0, 2.0, 3.0, 4.0, 5.0]
        while True:
            # self.get_logger().info('Publishing: "%s"' % msg.data)
            self.pub_str.publish(String(data=data_str))
            self.pub_int.publish(Int64(data=1))
            self.pub_int_array.publish(UInt64MultiArray(data=data_int_array))
            self.pub_float.publish(Float64(data=1.0))
            self.pub_float_array.publish(Float64MultiArray(data=data_float_array))
            time.sleep(self.delay_time)

    def change_delay_time(self):
        help_str="""
\033[80D w +0.01s
\033[80D s -0.01s
\033[80D q 退出
\033[80D
"""
        print(help_str)
        noblock_term = Noblock_terminal()
        time_stamp = time.time()
        while True:
            key = noblock_term.select_cmd(timeout=0.5)
            if key == 'w':
                self.delay_time += 0.01
            elif key == 's':
                self.delay_time -= 0.01
                if self.delay_time <= 0:
                    self.delay_time = 0.01
            elif key == 'q':
                break
            elif key == '0':
                continue
            else:
                print("\033[80D 未知指令")
            print(f"\033[80D delay_time: {self.delay_time:.2f}")
            # 定时打印 使用帮助
            cur_time = time.time()
            if int(cur_time - time_stamp) % 10 == 0:
                print(help_str)

        print("退出 控制")
        noblock_term.stop_no_block()

def main(args=None):
    rclpy.init(args=args)
    minimal_publisher = MinimalPublisher()
    rclpy.spin(minimal_publisher)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()